/*
 * Agent.java
 * RVO2 Library Java
 *
 * Copyright 2008 University of North Carolina at Chapel Hill
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * Please send all bug reports to <geom@cs.unc.edu>.
 *
 * The authors may be contacted via:
 *
 * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
 * Dept. of Computer Science
 * 201 S. Columbia St.
 * Frederick P. Brooks, Jr. Computer Science Bldg.
 * Chapel Hill, N.C. 27599-3175
 * United States of America
 *
 * <http://gamma.cs.unc.edu/RVO2/>
 */

package RVO;

import java.awt.*;
import java.text.DecimalFormat;
import java.util.List;
import java.util.ArrayList;
import java.util.Map;
import java.util.HashMap;

/**
 * Defines an agent in the simulation.
 */
public class Agent {
    List<Map.Entry<Float, Agent>> agentNeighbors_ = new ArrayList<Map.Entry<Float, Agent>>();
    List<Map.Entry<Float, Obstacle>> obstacleNeighbors_ = new ArrayList<Map.Entry<Float, Obstacle>>();
    List<Line> orcaLines_ = new ArrayList<Line>();
    Vector2 position_;
    Vector2 prefVelocity_;
    public float timeStep_;
    public String name;
    public Color color;
    Vector2 velocity_;
    int id_ = 0;
    int maxNeighbors_ = 0;
    float maxSpeed_ = 0.0f;
    float neighborDist_ = 0.0f;
    float radius_ = 0.0f;
    float timeHorizon_ = 0.0f;
    float timeHorizonObst_ = 0.0f;
    boolean needDelete_ = false;
    private Simulator simulator;

    public void setSimulator(Simulator simulator) {
        this.simulator = simulator;
    }

    private Vector2 newVelocity_;

    /**
     * Computes the neighbors of this agent.
     */
    void computeNeighbors() {
        obstacleNeighbors_.clear();
        float rangeSq = RVOMath.sqr(timeHorizonObst_ * maxSpeed_ + radius_);
        simulator.kdTree_.computeObstacleNeighbors(this, rangeSq);

        agentNeighbors_.clear();

        if (maxNeighbors_ > 0) {
            float[] rangeSqArray = {RVOMath.sqr(neighborDist_)};
            simulator.kdTree_.computeAgentNeighbors(this, rangeSqArray);
        }
    }

    /**
     * Computes the new velocity of this agent.
     */
    void computeNewVelocity() {
        orcaLines_.clear();

        float invTimeHorizonObst = 1.0f / timeHorizonObst_;

        /* Create obstacle ORCA lines. */
        for (int i = 0; i < obstacleNeighbors_.size(); ++i) {
            Obstacle obstacle1 = obstacleNeighbors_.get(i).getValue();
            Obstacle obstacle2 = obstacle1.next_;

            Vector2 relativePosition1 = Vector2.subtract(obstacle1.point_, position_);
            Vector2 relativePosition2 = Vector2.subtract(obstacle2.point_, position_);

            /*
             * Check if velocity obstacle of obstacle is already taken care
             * of by previously constructed obstacle ORCA lines.
             */
            boolean alreadyCovered = false;

            for (int j = 0; j < orcaLines_.size(); ++j) {
                if (RVOMath.det(Vector2.subtract(Vector2.multiply(relativePosition1, invTimeHorizonObst), orcaLines_.get(j).point), orcaLines_.get(j).direction) - invTimeHorizonObst * radius_ >= -RVOMath.RVO_EPSILON && RVOMath.det(Vector2.subtract(Vector2.multiply(relativePosition2, invTimeHorizonObst), orcaLines_.get(j).point), orcaLines_.get(j).direction) - invTimeHorizonObst * radius_ >= -RVOMath.RVO_EPSILON) {
                    alreadyCovered = true;
                    break;
                }
            }

            if (alreadyCovered) {
                continue;
            }

            /* Not yet covered. Check for collisions. */
            float distSq1 = RVOMath.absSq(relativePosition1);
            float distSq2 = RVOMath.absSq(relativePosition2);

            float radiusSq = RVOMath.sqr(radius_);

            Vector2 obstacleVector = Vector2.subtract(obstacle2.point_, obstacle1.point_);
            float s = Vector2.multiply(Vector2.negate(relativePosition1), obstacleVector) / RVOMath.absSq(obstacleVector);
            float distSqLine = RVOMath.absSq(Vector2.subtract(Vector2.negate(relativePosition1), Vector2.multiply(obstacleVector, s)));

            Line line = new Line();

            if (s < 0.0f && distSq1 <= radiusSq) {
                /* Collision with left vertex. Ignore if non-convex. */
                if (obstacle1.convex_) {
                    line.point = new Vector2(0.0f, 0.0f);
                    line.direction = RVOMath.normalize(new Vector2(-relativePosition1.y(), relativePosition1.x()));
                    orcaLines_.add(line);
                }
                continue;
            } else if (s > 1.0f && distSq2 <= radiusSq) {
                /*
                 * Collision with right vertex. Ignore if non-convex or if
                 * it will be taken care of by neighboring obstacle.
                 */
                if (obstacle2.convex_ && RVOMath.det(relativePosition2, obstacle2.direction_) >= 0.0f) {
                    line.point = new Vector2(0.0f, 0.0f);
                    line.direction = RVOMath.normalize(new Vector2(-relativePosition2.y(), relativePosition2.x()));
                    orcaLines_.add(line);
                }
                continue;
            } else if (s >= 0.0f && s < 1.0f && distSqLine <= radiusSq) {
                /* Collision with obstacle segment. */
                line.point = new Vector2(0.0f, 0.0f);
                line.direction = Vector2.negate(obstacle1.direction_);
                orcaLines_.add(line);
                continue;
            }

            /*
             * No collision. Compute legs. When obliquely viewed, both legs
             * can come from a single vertex. Legs extend cut-off line when
             * non-convex vertex.
             */

            Vector2 leftLegDirection, rightLegDirection;

            if (s < 0.0f && distSqLine <= radiusSq) {
                /*
                 * Obstacle viewed obliquely so that left vertex
                 * defines velocity obstacle.
                 */
                if (!obstacle1.convex_) {
                    /* Ignore obstacle. */
                    continue;
                }

                obstacle2 = obstacle1;

                float leg1 = RVOMath.sqrt(distSq1 - radiusSq);
                leftLegDirection = Vector2.divide(new Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1), distSq1);
                rightLegDirection = Vector2.divide(new Vector2(relativePosition1.x() * leg1 + relativePosition1.y() * radius_, -relativePosition1.x() * radius_ + relativePosition1.y() * leg1), distSq1);
            } else if (s > 1.0f && distSqLine <= radiusSq) {
                /*
                 * Obstacle viewed obliquely so that
                 * right vertex defines velocity obstacle.
                 */
                if (!obstacle2.convex_) {
                    /* Ignore obstacle. */
                    continue;
                }

                obstacle1 = obstacle2;

                float leg2 = RVOMath.sqrt(distSq2 - radiusSq);
                leftLegDirection = Vector2.divide(new Vector2(relativePosition2.x() * leg2 - relativePosition2.y() * radius_, relativePosition2.x() * radius_ + relativePosition2.y() * leg2), distSq2);
                rightLegDirection = Vector2.divide(new Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2), distSq2);
            } else {
                /* Usual situation. */
                if (obstacle1.convex_) {
                    float leg1 = RVOMath.sqrt(distSq1 - radiusSq);
                    leftLegDirection = Vector2.divide(new Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1), distSq1);
                } else {
                    /* Left vertex non-convex; left leg extends cut-off line. */
                    leftLegDirection = Vector2.negate(obstacle1.direction_);
                }

                if (obstacle2.convex_) {
                    float leg2 = RVOMath.sqrt(distSq2 - radiusSq);
                    rightLegDirection = Vector2.divide(new Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2), distSq2);
                } else {
                    /* Right vertex non-convex; right leg extends cut-off line. */
                    rightLegDirection = obstacle1.direction_;
                }
            }

            /*
             * Legs can never point into neighboring edge when convex
             * vertex, take cutoff-line of neighboring edge instead. If
             * velocity projected on "foreign" leg, no constraint is added.
             */

            Obstacle leftNeighbor = obstacle1.previous_;

            boolean isLeftLegForeign = false;
            boolean isRightLegForeign = false;

            if (obstacle1.convex_ && RVOMath.det(leftLegDirection, Vector2.negate(leftNeighbor.direction_)) >= 0.0f) {
                /* Left leg points into obstacle. */
                leftLegDirection = Vector2.negate(leftNeighbor.direction_);
                isLeftLegForeign = true;
            }

            if (obstacle2.convex_ && RVOMath.det(rightLegDirection, obstacle2.direction_) <= 0.0f) {
                /* Right leg points into obstacle. */
                rightLegDirection = obstacle2.direction_;
                isRightLegForeign = true;
            }

            /* Compute cut-off centers. */
            Vector2 leftCutOff = Vector2.multiply(Vector2.subtract(obstacle1.point_, position_), invTimeHorizonObst);
            Vector2 rightCutOff = Vector2.multiply(Vector2.subtract(obstacle2.point_, position_), invTimeHorizonObst);
            Vector2 cutOffVector = Vector2.subtract(rightCutOff, leftCutOff);

            /* Project current velocity on velocity obstacle. */

            /* Check if current velocity is projected on cutoff circles. */
            float t = obstacle1 == obstacle2 ? 0.5f : Vector2.multiply(Vector2.subtract(velocity_, leftCutOff), cutOffVector) / RVOMath.absSq(cutOffVector);
            float tLeft = Vector2.multiply(Vector2.subtract(velocity_, leftCutOff), leftLegDirection);
            float tRight = Vector2.multiply(Vector2.subtract(velocity_, rightCutOff), rightLegDirection);

            if ((t < 0.0f && tLeft < 0.0f) || (obstacle1 == obstacle2 && tLeft < 0.0f && tRight < 0.0f)) {
                /* Project on left cut-off circle. */
                Vector2 unitW = RVOMath.normalize(Vector2.subtract(velocity_, leftCutOff));

                line.direction = new Vector2(unitW.y(), -unitW.x());
                line.point = Vector2.add(leftCutOff, Vector2.multiply(unitW, radius_ * invTimeHorizonObst));
                orcaLines_.add(line);
                continue;
            } else if (t > 1.0f && tRight < 0.0f) {
                /* Project on right cut-off circle. */
                Vector2 unitW = RVOMath.normalize(Vector2.subtract(velocity_, rightCutOff));

                line.direction = new Vector2(unitW.y(), -unitW.x());
                line.point = Vector2.add(rightCutOff, Vector2.multiply(unitW, radius_ * invTimeHorizonObst));
                orcaLines_.add(line);
                continue;
            }

            /*
             * Project on left leg, right leg, or cut-off line, whichever is
             * closest to velocity.
             */
            float distSqCutoff = (t < 0.0f || t > 1.0f || obstacle1 == obstacle2) ? Float.POSITIVE_INFINITY : RVOMath.absSq(Vector2.subtract(velocity_, Vector2.add(leftCutOff, Vector2.multiply(cutOffVector, t))));
            float distSqLeft = tLeft < 0.0f ? Float.POSITIVE_INFINITY : RVOMath.absSq(Vector2.subtract(velocity_, Vector2.add(leftCutOff, Vector2.multiply(leftLegDirection, tLeft))));
            float distSqRight = tRight < 0.0f ? Float.POSITIVE_INFINITY : RVOMath.absSq(Vector2.subtract(velocity_, Vector2.add(rightCutOff, Vector2.multiply(rightLegDirection, tRight))));

            if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) {
                /* Project on cut-off line. */
                line.direction = Vector2.negate(obstacle1.direction_);
                line.point = Vector2.add(leftCutOff, Vector2.multiply(new Vector2(-line.direction.y(), line.direction.x()), radius_ * invTimeHorizonObst));
                orcaLines_.add(line);
                continue;
            }

            if (distSqLeft <= distSqRight) {
                /* Project on left leg. */
                if (isLeftLegForeign) {
                    continue;
                }

                line.direction = leftLegDirection;
                line.point = Vector2.add(leftCutOff, Vector2.multiply(new Vector2(-line.direction.y(), line.direction.x()), radius_ * invTimeHorizonObst));
                orcaLines_.add(line);
                continue;
            }

            /* Project on right leg. */
            if (isRightLegForeign) {
                continue;
            }

            line.direction = Vector2.negate(rightLegDirection);
            line.point = Vector2.add(rightCutOff, Vector2.multiply(new Vector2(-line.direction.y(), line.direction.x()), radius_ * invTimeHorizonObst));
            orcaLines_.add(line);
        }

        int numObstLines = orcaLines_.size();

        float invTimeHorizon = 1.0f / timeHorizon_;

        /* Create agent ORCA lines. */
        for (int i = 0; i < agentNeighbors_.size(); ++i) {
            Agent other = agentNeighbors_.get(i).getValue();

            Vector2 relativePosition = Vector2.subtract(other.position_, position_);
            Vector2 relativeVelocity = Vector2.subtract(velocity_, other.velocity_);
            float distSq = RVOMath.absSq(relativePosition);
            float combinedRadius = radius_ + other.radius_;
            float combinedRadiusSq = RVOMath.sqr(combinedRadius);

            Line line = new Line();
            Vector2 u;

            if (distSq > combinedRadiusSq) {
                /* No collision. */
                Vector2 w = Vector2.subtract(relativeVelocity, Vector2.multiply(relativePosition, invTimeHorizon));

                /* Vector from cutoff center to relative velocity. */
                float wLengthSq = RVOMath.absSq(w);
                float dotProduct1 = Vector2.multiply(w, relativePosition);

                if (dotProduct1 < 0.0f && RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq) {
                    /* Project on cut-off circle. */
                    float wLength = RVOMath.sqrt(wLengthSq);
                    Vector2 unitW = Vector2.divide(w, wLength);

                    line.direction = new Vector2(unitW.y(), -unitW.x());
                    u = Vector2.multiply(unitW, (combinedRadius * invTimeHorizon - wLength));
                } else {
                    /* Project on legs. */
                    float leg = RVOMath.sqrt(distSq - combinedRadiusSq);

                    if (RVOMath.det(relativePosition, w) > 0.0f) {
                        /* Project on left leg. */
                        line.direction = Vector2.divide(new Vector2(relativePosition.x() * leg - relativePosition.y() * combinedRadius, relativePosition.x() * combinedRadius + relativePosition.y() * leg), distSq);
                    } else {
                        /* Project on right leg. */
                        line.direction = Vector2.negate(Vector2.divide(new Vector2(relativePosition.x() * leg + relativePosition.y() * combinedRadius, -relativePosition.x() * combinedRadius + relativePosition.y() * leg), distSq));
                    }

                    float dotProduct2 = Vector2.multiply(relativeVelocity, line.direction);
                    u = Vector2.subtract(Vector2.multiply(line.direction, dotProduct2), relativeVelocity);
                }
            } else {
                /* Collision. Project on cut-off circle of time timeStep. */
                //float invTimeStep = 1.0f / Simulator.Instance.timeStep_;
                float invTimeStep = 1.0f / timeStep_;

                /* Vector from cutoff center to relative velocity. */
                Vector2 w = Vector2.subtract(relativeVelocity, Vector2.multiply(relativePosition, invTimeStep));

                float wLength = RVOMath.abs(w);
                Vector2 unitW = Vector2.divide(w, wLength);

                line.direction = new Vector2(unitW.y(), -unitW.x());
                u = Vector2.multiply(unitW, (combinedRadius * invTimeStep - wLength));
            }

            line.point = Vector2.add(velocity_, Vector2.multiply(u, 0.5f));
            orcaLines_.add(line);
        }

        Vector2[] newVelocityArray = {newVelocity_};
        int lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, newVelocityArray);
        newVelocity_ = newVelocityArray[0];

        if (lineFail < orcaLines_.size()) {
            linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, newVelocityArray);
            newVelocity_ = newVelocityArray[0];
        }
    }

    /**
     * Inserts an agent neighbor into the set of neighbors of this agent.
     *
     * @param agent A pointer to the agent to be inserted.
     * @param rangeSq The squared range around this agent.
     */
    void insertAgentNeighbor(Agent agent, float[] rangeSq) {
        if (this != agent) {
            float distSq = RVOMath.absSq(Vector2.subtract(position_, agent.position_));

            if (distSq < rangeSq[0]) {
                if (agentNeighbors_.size() < maxNeighbors_) {
                    agentNeighbors_.add(new Map.Entry<Float, Agent>() {
                        @Override
                        public Float getKey() { return distSq; }
                        @Override
                        public Agent getValue() { return agent; }
                        @Override
                        public Agent setValue(Agent value) { return null; }
                    });
                }

                int i = agentNeighbors_.size() - 1;

                while (i != 0 && distSq < agentNeighbors_.get(i - 1).getKey()) {
                    agentNeighbors_.set(i, agentNeighbors_.get(i - 1));
                    --i;
                }

                final float finalDistSq = distSq;
                final Agent finalAgent = agent;
                agentNeighbors_.set(i, new Map.Entry<Float, Agent>() {
                    @Override
                    public Float getKey() { return finalDistSq; }
                    @Override
                    public Agent getValue() { return finalAgent; }
                    @Override
                    public Agent setValue(Agent value) { return null; }
                });

                if (agentNeighbors_.size() == maxNeighbors_) {
                    rangeSq[0] = agentNeighbors_.get(agentNeighbors_.size() - 1).getKey();
                }
            }
        }
    }

    /**
     * Inserts a static obstacle neighbor into the set of neighbors of this agent.
     *
     * @param obstacle The number of the static obstacle to be inserted.
     * @param rangeSq The squared range around this agent.
     */
    void insertObstacleNeighbor(Obstacle obstacle, float rangeSq) {
        Obstacle nextObstacle = obstacle.next_;

        float distSq = RVOMath.distSqPointLineSegment(obstacle.point_, nextObstacle.point_, position_);

        if (distSq < rangeSq) {
            final float finalDistSq = distSq;
            final Obstacle finalObstacle = obstacle;

            int i = obstacleNeighbors_.size() - 1;

            while (i != 0 && distSq < obstacleNeighbors_.get(i - 1).getKey()) {
                obstacleNeighbors_.set(i, obstacleNeighbors_.get(i - 1));
                --i;
            }
            obstacleNeighbors_.set(i, new Map.Entry<Float, Obstacle>() {
                @Override
                public Float getKey() { return finalDistSq; }
                @Override
                public Obstacle getValue() { return finalObstacle; }
                @Override
                public Obstacle setValue(Obstacle value) { return null; }
            });
        }
    }

    /**
     * Updates the two-dimensional position and two-dimensional velocity of this agent.
     */
    void update() {
        velocity_ = newVelocity_;
        //Vector2 move = Vector2.multiply(velocity_, Simulator.Instance.timeStep_);
        Vector2 move = Vector2.multiply(velocity_, timeStep_);
        Vector2 newPos = Vector2.add(position_, move);

        double left = newVelocity_.x_;
        DecimalFormat df = new DecimalFormat("#.#####"); // 保留五位小数
        String lFormattedNumber = df.format(left);

        double right = newVelocity_.y_;
        String rFormattedNumber = df.format(right);
        System.out.printf("%s从 %s 移动至 %s    [%s,%s]\n", name, position_, newPos,
                lFormattedNumber, rFormattedNumber);
        position_ = newPos;
    }

    /**
     * Solves a one-dimensional linear program on a specified line
     * subject to linear constraints defined by lines and a circular constraint.
     *
     * @param lines Lines defining the linear constraints.
     * @param lineNo The specified line constraint.
     * @param radius The radius of the circular constraint.
     * @param optVelocity The optimization velocity.
     * @param directionOpt True if the direction should be optimized.
     * @param result A reference to the result of the linear program.
     * @return True if successful.
     */
    private boolean linearProgram1(List<Line> lines, int lineNo, float radius, Vector2 optVelocity, boolean directionOpt, Vector2[] result) {
        float dotProduct = Vector2.multiply(lines.get(lineNo).point, lines.get(lineNo).direction);
        float discriminant = RVOMath.sqr(dotProduct) + RVOMath.sqr(radius) - RVOMath.absSq(lines.get(lineNo).point);

        if (discriminant < 0.0f) {
            /* Max speed circle fully invalidates line lineNo. */
            return false;
        }

        float sqrtDiscriminant = RVOMath.sqrt(discriminant);
        float tLeft = -dotProduct - sqrtDiscriminant;
        float tRight = -dotProduct + sqrtDiscriminant;

        for (int i = 0; i < lineNo; ++i) {
            float denominator = RVOMath.det(lines.get(lineNo).direction, lines.get(i).direction);
            float numerator = RVOMath.det(lines.get(i).direction, Vector2.subtract(lines.get(lineNo).point, lines.get(i).point));

            if (RVOMath.fabs(denominator) <= RVOMath.RVO_EPSILON) {
                /* Lines lineNo and i are (almost) parallel. */
                if (numerator < 0.0f) {
                    return false;
                }
                continue;
            }

            float t = numerator / denominator;

            if (denominator >= 0.0f) {
                /* Line i bounds line lineNo on the right. */
                tRight = Math.min(tRight, t);
            } else {
                /* Line i bounds line lineNo on the left. */
                tLeft = Math.max(tLeft, t);
            }

            if (tLeft > tRight) {
                return false;
            }
        }

        if (directionOpt) {
            /* Optimize direction. */
            if (Vector2.multiply(optVelocity, lines.get(lineNo).direction) > 0.0f) {
                /* Take right extreme. */
                result[0] = Vector2.add(lines.get(lineNo).point, Vector2.multiply(lines.get(lineNo).direction, tRight));
            } else {
                /* Take left extreme. */
                result[0] = Vector2.add(lines.get(lineNo).point, Vector2.multiply(lines.get(lineNo).direction, tLeft));
            }
        } else {
            /* Optimize closest point. */
            float t = Vector2.multiply(lines.get(lineNo).direction, Vector2.subtract(optVelocity, lines.get(lineNo).point));

            if (t < tLeft) {
                result[0] = Vector2.add(lines.get(lineNo).point, Vector2.multiply(lines.get(lineNo).direction, tLeft));
            } else if (t > tRight) {
                result[0] = Vector2.add(lines.get(lineNo).point, Vector2.multiply(lines.get(lineNo).direction, tRight));
            } else {
                result[0] = Vector2.add(lines.get(lineNo).point, Vector2.multiply(lines.get(lineNo).direction, t));
            }
        }

        return true;
    }

    /**
     * Solves a two-dimensional linear program subject to linear
     * constraints defined by lines and a circular constraint.
     *
     * @param lines Lines defining the linear constraints.
     * @param radius The radius of the circular constraint.
     * @param optVelocity The optimization velocity.
     * @param directionOpt True if the direction should be optimized.
     * @param result A reference to the result of the linear program.
     * @return The number of the line it fails on, and the number of lines if successful.
     */
    private int linearProgram2(List<Line> lines, float radius, Vector2 optVelocity, boolean directionOpt, Vector2[] result) {
        if (directionOpt) {
            /*
             * Optimize direction. Note that the optimization velocity is of
             * unit length in this case.
             */
            result[0] = Vector2.multiply(optVelocity, radius);
        } else if (RVOMath.absSq(optVelocity) > RVOMath.sqr(radius)) {
            /* Optimize closest point and outside circle. */
            result[0] = Vector2.multiply(RVOMath.normalize(optVelocity), radius);
        } else {
            /* Optimize closest point and inside circle. */
            result[0] = optVelocity;
        }

        for (int i = 0; i < lines.size(); ++i) {
            if (RVOMath.det(lines.get(i).direction, Vector2.subtract(lines.get(i).point, result[0])) > 0.0f) {
                /* Result does not satisfy constraint i. Compute new optimal result. */
                Vector2 tempResult = result[0];
                if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) {
                    result[0] = tempResult;
                    return i;
                }
            }
        }

        return lines.size();
    }

    /**
     * Solves a two-dimensional linear program subject to linear
     * constraints defined by lines and a circular constraint.
     *
     * @param lines Lines defining the linear constraints.
     * @param numObstLines Count of obstacle lines.
     * @param beginLine The line on which the 2-d linear program failed.
     * @param radius The radius of the circular constraint.
     * @param result A reference to the result of the linear program.
     */
    private void linearProgram3(List<Line> lines, int numObstLines, int beginLine, float radius, Vector2[] result) {
        float distance = 0.0f;

        for (int i = beginLine; i < lines.size(); ++i) {
            if (RVOMath.det(lines.get(i).direction, Vector2.subtract(lines.get(i).point, result[0])) > distance) {
                /* Result does not satisfy constraint of line i. */
                List<Line> projLines = new ArrayList<Line>();
                for (int ii = 0; ii < numObstLines; ++ii) {
                    projLines.add(lines.get(ii));
                }

                for (int j = numObstLines; j < i; ++j) {
                    Line line = new Line();

                    float determinant = RVOMath.det(lines.get(i).direction, lines.get(j).direction);

                    if (RVOMath.fabs(determinant) <= RVOMath.RVO_EPSILON) {
                        /* Line i and line j are parallel. */
                        if (Vector2.multiply(lines.get(i).direction, lines.get(j).direction) > 0.0f) {
                            /* Line i and line j point in the same direction. */
                            continue;
                        } else {
                            /* Line i and line j point in opposite direction. */
                            line.point = Vector2.multiply(Vector2.add(lines.get(i).point, lines.get(j).point), 0.5f);
                        }
                    } else {
                        line.point = Vector2.add(lines.get(i).point, Vector2.multiply(lines.get(i).direction, RVOMath.det(lines.get(j).direction, Vector2.subtract(lines.get(i).point, lines.get(j).point)) / determinant));
                    }

                    line.direction = RVOMath.normalize(Vector2.subtract(lines.get(j).direction, lines.get(i).direction));
                    projLines.add(line);
                }

                Vector2 tempResult = result[0];
                Vector2[] newResult = {result[0]};
                if (linearProgram2(projLines, radius, new Vector2(-lines.get(i).direction.y(), lines.get(i).direction.x()), true, newResult) < projLines.size()) {
                    /*
                     * This should in principle not happen. The result is by
                     * definition already in the feasible region of this
                     * linear program. If it fails, it is due to small
                     * floating point error, and the current result is kept.
                     */
                    result[0] = tempResult;
                } else {
                    result[0] = newResult[0];
                }

                distance = RVOMath.det(lines.get(i).direction, Vector2.subtract(lines.get(i).point, result[0]));
            }
        }
    }
}
